/*
Arm Tracker and Glove Demo by Noah Zerkin (noazark AT gmail.com) 2009
Released under GNU GPLv3.0 http://www.opensource.org/licenses/gpl-3.0.html
*/

import processing.serial.*;
import processing.opengl.*;
/*
import com.vuzix.iWear;
com.vuzix.iWear vr920;
  */
Serial port;  // Create object from Serial class
String val;      // Data received from the serial port
//String portname = "/dev/tty.usbserial-A4001nRq";
//  String portname = "/dev/tty.usbserial-A9003PGL";
String portname = "/dev/tty.usbserial-A8007ZRk";
boolean running = true;

int counterMoCap = 0;
int counterSplitter = 0;
int[] counterParser = new int[2];
int drawCounter = 0;

byte[] inBuffer = new byte[28];

char trigger;
String[] reading = new String[20];
int[] buffX = new int[2];
int[] buffY = new int[2];
int[] buffZ = new int[2];
int[] buffGX = new int[2];
int[] buffGY = new int[2];
int[] buffGZ = new int[2];
int[] buffFlex = new int[2];
int[] buffGloveX = new int[2];
int[] buffGloveY = new int[2];
int[] buffGloveZ = new int[2];
float[] gloveXVal = new float[2];
float[] gloveYVal = new float[2];
float[] gloveZVal = new float[2];
float[] magX =  new float[2];
float[] magY =  new float[2];
float[] magZ =  new float[2];
int[] magXRaw =  new int[2];
int[] magYRaw =  new int[2];
int[] magZRaw =  new int[2];
//String[][] readings = new String[2][25];
float[][] shoulder = new float[2][3];
int[][][] accelFilter = new int[2][7][20];
int[] filterCycle  = new int[2];
int[] xRaw = new int[2];
int[] yRaw = new int[2];
int[] zRaw = new int[2];
int[] gxRaw = new int[2];
int[] gyRaw = new int[2];
int[] flexRaw = new int[2];
int[] gloveXRaw = new int[2];
int[] gloveYRaw = new int[2];
int[] gloveZRaw = new int[2];
float[] xVal = new float[2];
float[] yVal = new float[2];
float[] zVal = new float[2];
float[] flexVal = new float[2];
float[] xTilt = new float[2];                 
float[] yTilt = new float[2];
float[] zTilt = new float[2];
float[] xRad = new float[2];
float[] yRad = new float[2];
float[] flex = new float[2];
float[] lastFlex = new float[2];
float[] gx = new float[2];
float[] gy = new float[2];
float[] gxGy = new float[2];
float[] gxGyTheta = new float[2];
float[] yRot = new float[2];
int forearmLength = 75;
int upperArmLength = 75;
float[] elbowAngle = new float[2];
int[] timeDelta = new int[2];
boolean[] dataRead = new boolean[2];
boolean[] initialized = new boolean[2];
  
float x = 50;
float y = 100;

float[][] obj = new float[100][5];

//glove
int[][] thumb = new int[2][12];
int[][] fingerIndex = new int[2][8];
int[][] fingerMiddle = new int[2][8];
int[][] fingerRing = new int[2][8];
int[][] fingerPinky = new int[2][8];
int[][] wrist = new int[2][8];
float[] gloveX = new float[2];
float[] gloveY = new float[2];
float[] gloveZ = new float[2];
boolean thumbStickOn = false;

//chuck
int joyX = 0;
int joyY = 0;
int chuckX = 0;
int chuckY = 0;
int chuckZ = 0;
int chuckPitch = 0;
int chuckRoll = 0;
int buttonCBuff = 0;
int buttonZBuff = 0;
boolean buttonC = false;
boolean buttonZ = false;

//camera
float zoom = 300;
float camVert = 0.0;
float camHoriz = 0.0;
float camDist = 200;
int camX = 0;
int camY = 0;
int camZ = 200;
int hudX = 0;
int hudY = 0;
int hudZ = 0;
float eyeX = 0.0;
float eyeY = 0.0;
float eyeZ = 0.0;
float centerX = 0.0;
float centerY = 0.0;
float centerZ = 0.0;
boolean pov = false;
boolean vid = false;
boolean worldBox = false;
boolean menu = false;
boolean reticle = false;
boolean pressed = true;
float vr920roll  = 0.0;
float vr920pitch = 0.0;
float vr920yaw   = 0.0;

//IMU Calc
int zchipLow = 285;
int zchipHigh = 400;
int xchipLow = 285;
int xchipHigh = 400;
int ychipLow = 285;
int ychipHigh = 400;
float xm = 2.0/((float)xchipHigh - (float)xchipLow);
float ym = 2.0/((float)ychipHigh - (float)ychipLow);
float zm = 2.0/((float)zchipHigh - (float)zchipLow);
float xb = 1.0 - (xm*(float)xchipHigh);
float yb = 1.0 - (ym*(float)ychipHigh);
float zb = 1.0 - (zm*(float)zchipHigh);
float[] xa = new float[2];
float[] ya = new float[2];
float[] za = new float[2];

//Glove Calc
int[] gloveZChipLow = new int[2];
int[] gloveZChipHigh = new int[2];
int[] gloveXChipLow = new int[2];
int[] gloveXChipHigh = new int[2];
int[] gloveYChipLow = new int[2];
int[] gloveYChipHigh = new int[2];
float[] glovePitch = new float[2];
float[] gloveRoll = new float[2];
float[] gloveZTilt = new float[2];
int[] flex90 = new int[2];
int[] flex180 = new int[2];

float[] gXMap = new float[2];
float[] gYMap = new float[2];
float[] gZMap = new float[2];

boolean body = true;

//New Arm Calc Proto
float[] a2hH = new float[2];
float[] a2hR = new float[2];
float[] a2hZFF = new float[2];
float[] a2hAngle = new float[2];
float[] a2hHx = new float[2];
float[] a2hHy = new float[2];
float[] a2hF = new float[2];
float[] a2hEx = new float[2];
float[] a2hEy = new float[2];
float[] a2hEz = new float[2];

//Gyro
int[] gxZero = new int[2];
int[] gyZero = new int[2];
int[] gyroScaleX = new int[2];
int[] gyroScaleY = new int[2];
boolean gyro = false;

//MoveableObject
boolean grabbed = false;
int grabbedObjRadius = 0;
boolean grabMenuSphere = false;
boolean intersectMenuSphere = false;
boolean previousMenuSphereGrabState = true;
int lastObjID = 1;
int grabbedObjID = 0;
int[] atomicVector = {1,1,1};

MovableSphereObj sphere1 = new MovableSphereObj(100,10,-50,10,#FF0000,#ADFF2F,#008000);
MovableSphereObj sphere2 = new MovableSphereObj(100,0.0,-20,10,#FF0000,#ADFF2F,#008000);
MovableSphereObj sphere3 = new MovableSphereObj(100,0.0,10,10,#FF0000,#ADFF2F,#008000);
MovableSphereObj sphere4 = new MovableSphereObj(100,0.0,30,10,#FF0000,#ADFF2F,#008000);
AtomicSphere atomSphere1 = new AtomicSphere(100,0.0,30,10,#FF0000,#ADFF2F,#008000, atomicVector);
AtomicSphere atomSphere2 = new AtomicSphere(0.0,100.0,30,10,#ADFF2F,#0000FF,#008000, atomicVector);
AtomicSphere atomSphere3 = new AtomicSphere(100,0.0,-50,10,#0000FF,#ADFF2F,#008000, atomicVector);
AtomicSphere atomSphere4 = new AtomicSphere(100,25,30,10,#00FF00,#ADFF2F,#008000, atomicVector);
MenuObj mainMenu = new MenuObj("Vision","MoCap","Enviro","Tools");

//Box Array
float boxX = 0;
float boxY = 0;
float boxZ = 0;

//ColorTable
color[] colorTable = new color[10];

//True Space Hand Tracking
float fingerTipX = 0.0;
float fingerTipY = 0.0;
float fingerTipZ = 0.0;
float[] grabbedObjectX = new float[2];
float[] grabbedObjectY = new float[2];
float[] grabbedObjectZ = new float[2];
float holdSpace = 0.0;

//Box Declarations

boolean drawing = false;
boolean drawTool = false;
boolean editing = false;
boolean fist = false;
boolean rock = false;
boolean hush = false;
boolean faceselect = false;
float[] drawBoxStart = new float[3];
float[] drawBoxEnd = new float[3];
float drawBoxHeight = 0.0;
float drawBoxWidth = 0.0;
float drawBoxDepth = 0.0;
float palmX = 0.0;
float palmY = 0.0;
float palmZ = 0.0;

ArrayList boxes;
ArrayList bullets;
ArrayList targets;
ArrayList atomics;
Gun pistol = new Gun(75,0,20);
int myPose = 1111;
float realGloveRoll = 0;
float realGlovePitch = 0;
float wristTwist = 0;

//Modes
boolean drawMode = false;
boolean gameMode = false;
boolean cameraMode = false;

WeightedFilter xFilter = new WeightedFilter();
WeightedFilter yFilter = new WeightedFilter();
WeightedFilter zFilter = new WeightedFilter();
WeightedFilter flexFilter = new WeightedFilter();
WeightedFilter gloveXFilter = new WeightedFilter();
WeightedFilter gloveYFilter = new WeightedFilter();
WeightedFilter gloveZFilter = new WeightedFilter();
WeightedFilter magXFilter = new WeightedFilter();
WeightedFilter magYFilter = new WeightedFilter();
WeightedFilter magZFilter = new WeightedFilter();
boolean magVis = false;

color backColor = 000;

void setup() {
  bullets = new ArrayList();
  boxes = new ArrayList();
  targets = new ArrayList();
  atomics = new ArrayList();
  //size(1400, 850, OPENGL);
gloveZChipLow[0] = 38;
gloveZChipHigh[0] = 217;
gloveXChipLow[0] = 38;
gloveXChipHigh[0] = 217;
gloveYChipLow[0] = 38;
gloveYChipHigh[0] = 217;
flex90[0] = 100;
flex180[0] = 255;

dataRead[0] = false;
dataRead[1] = false;
initialized[0] = false;
initialized[1] = false;
counterParser[0] = 0;
counterParser[1] = 0;

gx[0] = 0.0;
gy[0] = 0.0;
gxGy[0] = 0.0;
gxGyTheta[0] = 0.0;
gx[1] = 0.0;
gy[1] = 0.0;
gxGy[1] = 0.0;
gxGyTheta[1] = 0.0;

gxZero[0] = 408;
gyZero[0] = 488;
gyroScaleX[0] = 55;
gyroScaleY[0] = 55;

gloveZChipLow[1] = 38;
gloveZChipHigh[1] = 217;
gloveXChipLow[1] = 38;
gloveXChipHigh[1] = 217;
gloveYChipLow[1] = 38;
gloveYChipHigh[1] = 217;
flex90[1] = 100;
flex180[1] = 255;

gxZero[1] = 408;
gyZero[1] = 488;
gyroScaleX[1] = 55;
gyroScaleY[1] = 55;
yRot[0] = 0;
yRot[1] = 0;

buffGX[0] = 0;
buffGY[0] = 0;
buffGX[1] = 0;
buffGY[1] = 0;

  size(1024, 768, OPENGL);
//    size(1280, 720, OPENGL);
  port = new Serial(this, portname, 57600);
//  Thread moCap1 = new MoCap();
//  Thread parser0 = new Parser(0);
//  Thread parser1 = new Parser(1);
//  Thread splitter1 = new Splitter();
  fill(204);
  /*
  vr920 = new com.vuzix.iWear();
  vr920.Start();
  vr920.Open();
  vr920.SetSmoothing( 32 );
  */
  shoulder[0][0] = 0;
  shoulder[0][1] = 0;
  shoulder[0][2] = 0;
  shoulder[1][0] = 0;
  shoulder[1][1] = 0;
  shoulder[1][2] = -120;
  obj[0][0] = 1;
  obj[0][1] = 1;
  obj[0][2] = 0.0;
  obj[0][3] = 0.0;
  obj[0][4] = 0.0;
  colorTable[0] = #FF0000;
  colorTable[1] = #ADFF2F;
  colorTable[2] = #008000;
  for(int i = 0; i<20; i++){
    reading[i] = "a 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 b";
  }
  sphereDetail(20);
//       myCapture.settings();
  
  PFont font;
  font = loadFont("CourierNewPSMT-24.vlw"); 
  textFont(font);
  port.bufferUntil(0);
  port.write('a');
  buttonC = true;
  atomSphere1.activate();
    atomSphere2.activate();
      atomSphere3.activate();
        atomSphere4.activate();
          background(backColor);
}

void draw()
  {
   // colorMode(RGB, 1.0);
      ambientLight(102, 102, 102);
lightSpecular(204, 204, 204);
directionalLight(102, 102, 256, 0, .5, .5);
//print(degrees(gx));print(" | ");print(degrees(gy));print(" | ");print(degrees(xTilt));print(" | ");print(degrees(yTilt));print(" | ");print(degrees(zTilt));print(" | ");print(timeDelta);print(" | ");print(degrees(gxGy));print(" | ");println(degrees(gxGyTheta));
//if(gx<0){print("-");}else{print("+");}if(gy<0){println("-");}else{println("+");}
//print(gxRaw);print(" | ");println(gyRaw);
  background(backColor);
//  if(backColor < 999){
//    backColor += 5;
//  }
 // else{backColor = 0;}
  fill(204);
  gloveTilt(1);
  fingerMap(1);
  if(fingerIndex[0][1] > 45){
    buttonC = true;
  }
  else{buttonC = false;}
  if(thumbStickOn){
    thumbStick(0);
  }
  zeroGyro();
  camControl();
  if(!pov){
  camera(camX, camY, camZ, // eyeX, eyeY, eyeZ
         centerX, centerY, centerZ, // centerX, centerY, centerZ
         0.0, 1.0, 0.0); // upX, upY, upZ
  }
  else{
  camera(eyeX, eyeY, eyeZ, // eyeX, eyeY, eyeZ
         camX, camY, camZ, // centerX, centerY, centerZ
         0.0, 1.0, 0.0); // upX, upY, upZ
  }
  drawGrid(2000,100,300);
  if(body){
  drawBody();
  pushMatrix();
  if(gyro){
    rotateY(yRot[0]);
  }
  armR(0);
  popMatrix();
  pushMatrix();
  translate(shoulder[1][0], shoulder[1][1], shoulder[1][2]);
  if(gyro){
    rotateY(yRot[1]);
  }
  //armL(1);
  popMatrix();
  }
  sphere1.update();
  sphere2.update();
  sphere3.update();
  sphere4.update();
  atomSphere1.update();
    atomSphere2.update();
      atomSphere3.update();
        atomSphere4.update();
  menuSphere(5,125,-20,0);
    if(magVis){
          pushMatrix();
          translate(hudX,hudY,hudZ);
          rotateY(HALF_PI+radians(vr920yaw));
          rotateX(-radians(vr920pitch));
          translate(-320,-240,0);
          stroke(#ADFF2F);
          line(320, 0,0, 320, 480, 0);
          line(0, 240,0, 640, 240, 0);
          stroke(#0000FF);
          ellipse(magX[0], 30, 25, 25);
          stroke(#FF0000);
          ellipse(magY[0], 60, 25, 25);
          stroke(#00FF00);
          ellipse(magZ[0], 100, 25, 25);
          print(magX[0]);print(" - ");print(magY[0]);print(" - ");println(magZ[0]);
       popMatrix();
  }
  if(vid){
    pushMatrix();
          translate(camX,camY,camZ);
          rotateY(HALF_PI+radians(vr920yaw));
          rotateX(-radians(vr920pitch));
//          image(myCapture, -3200,-2400,6400,4800);
  popMatrix();
  if(magVis){
          pushMatrix();
          translate(hudX,hudY,hudZ);
          rotateY(HALF_PI+radians(vr920yaw));
          rotateX(-radians(vr920pitch));
          translate(-320,-240,0);
          line(320, 0,0, 320, 480, 0);
          line(0, 240,0, 640, 240, 0);
       popMatrix();
  }
//  if(myron){
//      pushMatrix();
//          translate(hudX,hudY,hudZ);
//          rotateY(HALF_PI+radians(vr920yaw));
//          rotateX(-radians(vr920pitch));
//          translate(-320,-240,0);
//          myron();
//       popMatrix();
//  }
  }
  if(mainMenu.objStatus() == true){
    mainMenu.update();
  }
//commented
 if(!pistol.holding() && fingerIndex[0][0] <= 10 && fingerIndex[0][1] <= 20 && fingerMiddle[0][0] >= 25 && fingerMiddle[0][1] >=60 && fingerRing[0][0] >= 25 && fingerRing[0][1] >=60 && fingerPinky[0][0] >= 25 && fingerPinky[0][1] >=60){
   drawTool = true;
 }
 else{
   drawTool = false;
 }
 
 if(drawTool && !drawing){
   drawing = true;
   drawBoxStart[0] = obj[0][2];
   drawBoxStart[1] = obj[0][3];
   drawBoxStart[2] = obj[0][4];
 }
 if(drawTool && drawing){
 pushMatrix();
   translate(drawBoxStart[0], drawBoxStart[1], drawBoxStart[2]);
   drawBoxHeight = modelY(0,0,0) - obj[0][3];
   drawBoxWidth = modelX(0,0,0) - obj[0][2];
   drawBoxDepth = modelZ(0,0,0) - obj[0][4];
   translate(-drawBoxWidth/2, -drawBoxHeight/2, -drawBoxDepth/2);
   noFill();
   stroke(#ADFF2F);
   box(drawBoxWidth, drawBoxHeight, drawBoxDepth);
 popMatrix();
 }
 if(drawing && !drawTool){
   drawBoxEnd[0] = obj[0][2];
   drawBoxEnd[1] = obj[0][3];
   drawBoxEnd[2] = obj[0][4];
   pushMatrix();
   translate(drawBoxStart[0], drawBoxStart[1], drawBoxStart[2]);
   drawBoxWidth = modelX(0,0,0) - drawBoxEnd[0];
   drawBoxHeight = modelY(0,0,0) - drawBoxEnd[1];
   drawBoxDepth = modelZ(0,0,0) - drawBoxEnd[2];
   drawing = false;
   boxes.add(new PersistantBox(drawBoxStart[0]-drawBoxWidth/2, drawBoxStart[1]-drawBoxHeight/2, drawBoxStart[2]-drawBoxDepth/2, drawBoxWidth, drawBoxHeight, drawBoxDepth));   
 }
 if(fingerIndex[0][0] >= 25 && fingerIndex[0][1] >= 60 && fingerMiddle[0][0] >= 25 && fingerMiddle[0][1] >=60 && fingerRing[0][0] >= 25 && fingerRing[0][1] >=60 && fingerPinky[0][0] >= 25 && fingerPinky[0][1] >=60){
        fist = true;
 }
 else{
   fist = false;
 }
 ///*
 if(fingerIndex[0][0] <= 20 && fingerIndex[0][1] <= 20 && fingerMiddle[0][0] <=20 && fingerMiddle[0][1] <=20 && fingerRing[0][0] >= 15 && fingerRing[0][1] >=50 && fingerPinky[0][0] >=15 && fingerPinky[0][1] >=50){
        hush = true;
 }
 else{
   hush = false;
 }
// */
  if(fingerIndex[0][0] <= 10 && fingerIndex[0][1] <= 20 && fingerMiddle[0][0] >= 25 && fingerMiddle[0][1] >=60 && fingerRing[0][0] >= 25 && fingerRing[0][1] >=60 && fingerPinky[0][0] <= 10 && fingerPinky[0][1] <=20){
        rock = true;
 }
 else{
   rock = false;
 }
 if(rock){
      gxZero[0] = gxRaw[0]; 
      gyZero[0] = gyRaw[0]; 
      gxZero[1] = gxRaw[1]; 
      gyZero[1] = gyRaw[1];
     yRot[0] = 0.0; 
 }
 
 if(!drawing && !drawTool){
   pushMatrix();
   translate(drawBoxStart[0]-drawBoxWidth/2, drawBoxStart[1]-drawBoxHeight/2, drawBoxStart[2]-drawBoxDepth/2);
   noFill();
   stroke(#ADFF2F);
   box(drawBoxWidth, drawBoxHeight, drawBoxDepth);
   popMatrix();
 }
 
   if((palmX >= drawBoxStart[0] && palmX <= drawBoxStart[0] || palmX <= drawBoxStart[0] && palmX >= drawBoxStart[0]-10)
   && (palmY >= drawBoxStart[1] && palmY <= drawBoxEnd[1] || palmY <= drawBoxStart[1] && palmY >= drawBoxEnd[1])
   && (palmZ >= drawBoxStart[2] && palmZ <= drawBoxEnd[2] || palmZ <= drawBoxStart[2] && palmZ >= drawBoxEnd[2])){
     faceselect = true;
   }
   else{faceselect = false;}
   if(faceselect && fist){
     editing = true;
   }
   if(faceselect || editing){
     if(editing){
       fill(#ADFF2F, 90);
     }
     pushMatrix();
     translate(drawBoxStart[0], drawBoxStart[1]-drawBoxHeight/2, drawBoxStart[2]-drawBoxDepth/2);
       drawBoxWidth = drawBoxStart[0] - drawBoxEnd[0];
       box(5, drawBoxHeight,drawBoxDepth);
     popMatrix();
   }
   if(editing && fist){
       drawBoxStart[0] = palmX;
       drawBoxWidth = drawBoxStart[0] - drawBoxEnd[0];
   }
   if(editing && !fist){
     editing = false;
   }
  
//  drawCounter+=1;
//  print("Draw - "); println(drawCounter);
  for (int i = boxes.size()-1; i >= 0; i--) { 
    // An ArrayList doesn't know what it is storing so we have 
    // to cast the object coming out
    PersistantBox drawnBox = (PersistantBox) boxes.get(i);
    drawnBox.display();
  }
  byte[][] fingers = new byte[4][2];
  fingers[0][0] = byte(fingerIndex[0][0]);
  fingers[0][1] = byte(fingerIndex[0][1]);
  fingers[1][0] = byte(fingerMiddle[0][0]);
  fingers[1][1] = byte(fingerMiddle[0][1]);
  fingers[2][0] = byte(fingerRing[0][0]);
  fingers[2][1] = byte(fingerRing[0][1]);
  fingers[3][0] = byte(fingerPinky[0][0]);
  fingers[3][1] = byte(fingerPinky[0][1]);
  myPose = handPose(fingers);
//  println(myPose);

  pushMatrix();
  if(!pistol.holding()){
  pistol.display();
  }
 
  popMatrix();
  
  bulletTrack();
  targetTrack();
  if(hush){
  hushNav();
  }
  else{joyX = 0; joyY = 0;}
}

void serialEvent(Serial port){
  port.readBytesUntil(0, inBuffer);
  if(port.available() > 29){
    port.clear();
  }
  else{
    
  buffGloveX[0] = int(inBuffer[0]);
  buffGloveY[0] = int(inBuffer[1]);
  buffGloveZ[0] = int(inBuffer[2]);
  //  print(buffGloveX[0]); print(" - ");  print(buffGloveY[0]); print(" - ");  println(buffGloveZ[0]);
  buffX[0] = int(inBuffer[3]);
  buffY[0] = int(inBuffer[4]);
  buffZ[0] = int(inBuffer[5]);
  buffFlex[0] = int(inBuffer[6]);
  buffGY[0] = int(inBuffer[7]);
  buffGX[0] = int(inBuffer[8]);
  buffGZ[0] = int(inBuffer[9]);
  timeDelta[0] = int(inBuffer[10]);
  thumb[0][11] = int(inBuffer[11]);
  thumb[0][10] = int(inBuffer[12]);
  thumb[0][9] = int(inBuffer[13]);
  fingerIndex[0][6] = int(inBuffer[14]);
  fingerIndex[0][7] = int(inBuffer[15]);
  fingerMiddle[0][6] = int(inBuffer[16]);
  fingerMiddle[0][7] = int(inBuffer[17]);
  fingerRing[0][6] = int(inBuffer[18]);
  fingerRing[0][7] = int(inBuffer[19]);
  fingerPinky[0][6] = int(inBuffer[20]);
  fingerPinky[0][7] = int(inBuffer[21]);
  wrist[0][6] = int(inBuffer[22]);
  wrist[0][7] = int(inBuffer[23]);
  magXRaw[0] = int(inBuffer[24]);
  magYRaw[0] = int(inBuffer[25]);
  magZRaw[0] = int(inBuffer[26]);
  parseAndFilter(0);
  IMUcalc(0);
  gloveTilt(0);
  fingerMap(0);
          /*
  dataRead[0] = false;
  initialized[0] = true;
  */
  }
}

void bulletTrack(){
   for(int i = bullets.size()-1; i >= 0; i--) {
    Bullet flyingLead = (Bullet)bullets.get(i);
    float[] leadVector = flyingLead.getVector();
    float[] leadPosition = flyingLead.getPosition();
  /*  for(int j = 0; j < 3; j++){
      print(leadPosition[j]); print(" - ");
    }
    */  
    leadPosition[0] += leadVector[0];
    leadPosition[1] += leadVector[1];
    leadPosition[2] += leadVector[2];
    float[] newLeadPosition = new float[3];
    for(int j = 0; j < 3; j++){
      newLeadPosition[j] = leadPosition[j];
    }
    Bullet newFlyingLead = (new Bullet(newLeadPosition, leadVector));
    bullets.set(i, newFlyingLead);
    newFlyingLead.display();
    if(!newFlyingLead.isFlying()){
      bullets.remove(i);
    }
  }
}

void targetTrack(){
  sphereDetail(5);
   for(int i = targets.size()-1; i >= 0; i--) {
    Target flyingTarget = (Target)targets.get(i);
    Target newTarget = (new Target(flyingTarget.getOrbitRadius(), flyingTarget.getAltitude(), flyingTarget.getOrbitPosition(), flyingTarget.getTargetSpeed(), flyingTarget.getTargetRadius()));
        newTarget.update();
    targets.set(i, newTarget);
    newTarget.display();
    if(newTarget.isHit()){
      targets.remove(i);
    }
  }
  sphereDetail(20);
}

void createTargets(){
    targets.add(new Target(1000, 500, 0, 0.01, 50));
    targets.add(new Target(2000, 300, 0, 0.001, 50));
    targets.add(new Target(600, -30, 0, -0.005, 30));
    targets.add(new Target(1500, 900, 0, 0.02, 50));
    targets.add(new Target(1750, 500, 0, -0.01, 80));
}

void clearTargets(){
      for (int i = targets.size()-1; i >= 0; i--) {
        targets.remove(i);
      }
}

class PersistantBox{
  
  float[] pBoxCoords = new float[3];
  float[] pBoxWHD = new float[3];
  
  
  PersistantBox(float pBoxX, float pBoxY, float pBoxZ, float pBoxW, float pBoxH, float pBoxD){
    pBoxCoords[0] = pBoxX; pBoxCoords[1] = pBoxY; pBoxCoords[2] = pBoxZ;
    pBoxWHD[0] = pBoxW; pBoxWHD[1] = pBoxH; pBoxWHD[2] = pBoxD;
  }
  
  void display(){
    pushMatrix();
    translate(pBoxCoords[0],pBoxCoords[1],pBoxCoords[2]);
    noFill();
    box(pBoxWHD[0], pBoxWHD[1], pBoxWHD[2]);
    popMatrix();
  }
}

void drawBody(){
  stroke(255);
  strokeWeight(10);
//  line(shoulder[0][0],shoulder[0][1],shoulder[0][2],shoulder[1][0],shoulder[1][1],shoulder[1][2]);
//  line(shoulder[0][0],shoulder[0][1]-60,-60,shoulder[0][0],shoulder[0][1]+200,-60);
  strokeWeight(2);
  pushMatrix();
  translate(0,shoulder[0][1]-60,-60);
 // noStroke();
  sphere(25);
  eyeX = modelX(12.5,0,0);
  eyeY = modelY(12.5,0,0);
  eyeZ = modelZ(12.5,0,0);
  popMatrix();
}

void menuSphere(int radius, int menuSphereX, int menuSphereY, int menuSphereZ){
    if(dist(obj[0][2],obj[0][3],obj[0][4],menuSphereX,menuSphereY,menuSphereZ) <= radius){
      intersectMenuSphere = true;
    }
    else{
      intersectMenuSphere = false;
    }
    if(buttonC && intersectMenuSphere){
      grabMenuSphere = true;
    }
    else{
      grabMenuSphere = false;
    }
    if(grabMenuSphere){
      if(grabMenuSphere != previousMenuSphereGrabState){
        if(menu){
          menu = false;
        }
        else{
          menu = true;
        }
      }
    }
  if(intersectMenuSphere){
    if(grabMenuSphere){
      stroke(#ADFF2F);
      fill(#FF0000);
    }
    else{
    stroke(#FF0000);
    fill(#ADFF2F);
    }
  }
  else{
    noStroke();
    //   stroke(#ADFF2F);
    fill(#ADFF2F);
  }
  pushMatrix();
  translate(menuSphereX,menuSphereY,menuSphereZ);
  sphere(radius);
  popMatrix();
  previousMenuSphereGrabState = grabMenuSphere;
  if(menu){
    menuObj(menuSphereX, menuSphereY);
  }
}
  
void menuObj(int menuSphereX, int menuSphereY){
  pushMatrix();
  noStroke();
  translate(menuSphereX,menuSphereY-30,0);
  rotateY(HALF_PI);
  //Window
  fill(#0000FF, 50);
//  rect(0,-40,100,80);
  //Button
  fill(#0000FF, 90);
  stroke(#0000FF, 100);
  translate(10,-30,-5);
  boxX = modelX(0,0,0); boxY = modelY(0,0,0); boxZ = modelZ(0,0,0);
  if(obj[0][2] <= boxX+5 && obj[0][2] >= boxX-5 && obj[0][3] <= boxY+5 && obj[0][3] >= boxY-5 && obj[0][4] <= boxZ+5 && obj[0][4] >= boxZ-5){
    fill(#ADFF2F, 90);
    sphere1.activate();
  }
  box(10);
    if (sphere1.sphereStatus() == true){
    fill(#ADFF2F, 20);
      sphere(7.5);
  }
  fill(#0000FF, 90);
  translate(0,15,0);  boxX = modelX(0,0,0); boxY = modelY(0,0,0); boxZ = modelZ(0,0,0);
  if(obj[0][2] <= boxX+5 && obj[0][2] >= boxX-5 && obj[0][3] <= boxY+5 && obj[0][3] >= boxY-5 && obj[0][4] <= boxZ+5 && obj[0][4] >= boxZ-5){
    fill(#ADFF2F, 90);
      sphere2.activate();
  }
    box(10);
  if (sphere2.sphereStatus() == true){
    fill(#ADFF2F, 20);
      sphere(7.5);
  }
  fill(#0000FF, 90);
  translate(0,15,0);  boxX = modelX(0,0,0); boxY = modelY(0,0,0); boxZ = modelZ(0,0,0);
  if(obj[0][2] <= boxX+5 && obj[0][2] >= boxX-5 && obj[0][3] <= boxY+5 && obj[0][3] >= boxY-5 && obj[0][4] <= boxZ+5 && obj[0][4] >= boxZ-5){
    fill(#ADFF2F, 90);
      sphere3.activate();
  }
  box(10);
    if (sphere3.sphereStatus() == true){
    fill(#ADFF2F, 20);
      sphere(7.5);
  }
  fill(#0000FF, 90);
  translate(0,15,0);  boxX = modelX(0,0,0); boxY = modelY(0,0,0); boxZ = modelZ(0,0,0);
  if(obj[0][2] <= boxX+5 && obj[0][2] >= boxX-5 && obj[0][3] <= boxY+5 && obj[0][3] >= boxY-5 && obj[0][4] <= boxZ+5 && obj[0][4] >= boxZ-5){
    fill(#ADFF2F, 90);
    sphere4.activate();
  }
  box(10);
    if (sphere4.sphereStatus() == true){
    fill(#ADFF2F, 20);
      sphere(7.5);
  }
    for(int i=0;i<4;i+=1){
  translate(15,0,0);
  pushMatrix();
    fill(#0000FF, 90);
    boxX = modelX(0,0,0); boxY = modelY(0,0,0); boxZ = modelZ(0,0,0);
    if(obj[0][2] <= boxX+5 && obj[0][2] >= boxX-5 && obj[0][3] <= boxY+5 && obj[0][3] >= boxY-5 && obj[0][4] <= boxZ+5 && obj[0][4] >= boxZ-5){
      fill(#ADFF2F, 90);
    }
    box(10);
  for(int j=0;j<3;j+=1){
    fill(#0000FF, 90);
    translate(0,-15,0);  boxX = modelX(0,0,0); boxY = modelY(0,0,0); boxZ = modelZ(0,0,0);
    if(obj[0][2] <= boxX+5 && obj[0][2] >= boxX-5 && obj[0][3] <= boxY+5 && obj[0][3] >= boxY-5 && obj[0][4] <= boxZ+5 && obj[0][4] >= boxZ-5){
      fill(#ADFF2F, 90);
    }
    box(10);
  }
  popMatrix();
  }
  popMatrix();
  fill(200);
}

class VRObj{
  float[] mObj = new float[3];
  int objID = lastObjID + 1;
  boolean active = false;
  
  VRObj(){
    lastObjID += 1;
  }
  
  void activate(boolean on){
    active = on;
  }
  
  boolean objStatus(){
    return active;
  }
  
}
     
class MovableSphereObj {
  float[] mObj = new float[3];
  boolean intersect = false;
  boolean grab = false;
  int objID = lastObjID + 1;
  int objRadius = 0;
  color objNormalColor = #FF0000; color objOverColor = #FF0000; color objGrabColor = #FF0000;
  boolean active = false;
  boolean activated = false;
  
  MovableSphereObj(float x, float y, float z, int radius, color normalColor, color overColor, color grabColor){
    lastObjID += 1; mObj[0] = x; mObj[1] = y; mObj[2] = z; objRadius = radius; objNormalColor = normalColor; objOverColor = overColor; objGrabColor = grabColor;
  }
  
   void activate(){
    if(active && !activated && buttonC){
     activated = true;
     active = false;
    }
   if(!active && !activated && buttonC){
    activated = true;
    active = true;
   }
   if(activated && !buttonC){
     activated = false;
   }
  }
  
  boolean sphereStatus(){
    return active;
  }
  
    void update(){
     if(active){
      if(dist(obj[0][2],obj[0][3],obj[0][4],mObj[0],mObj[1],mObj[2]) <= objRadius){
//  if(obj[0][1] <= mObj[0] + ((.5*radius)+2.5) && obj[0][1] >= mObj[0] - ((.5*radius)+2.5) && obj[0][2] <= mObj[1] + ((.5*radius)+2.5) && obj[0][2] >= mObj[1] - ((.5*radius)+2.5) && obj[0][3] <= mObj[2] + ((.5*radius)+2.5) && obj[0][3] >= mObj[2] - ((.5*radius)+2.5)){
        intersect = true;
      }
      else{
        intersect = false;
      }
      if(!buttonC){grab=false;}
      if(intersect){
        if(buttonC){
          if(!grabbed && grabbedObjID != objID){
            grab = true;
            grabbed = true;
            grabbedObjID = objID;
          }
          else{
            if(grabbed && grabbedObjID == objID){
              grab = true;
            }
            else{
              if(!grabbed && grabbedObjID == objID){
                grab = true;
                grabbed = true;
              }
              else{
                if(grabbed && grabbedObjID != objID){
                  grab = false;
                }
              }
            }
          }
        }
        else{
          grab = false;
        }
      }
      if(!grab && grabbedObjID == objID){
        grabbed = false;
      }
      if(grab && grabbedObjID == objID){
        mObj[0] = grabbedObjectX[0];
        mObj[1] = grabbedObjectY[0];
        mObj[2] = grabbedObjectZ[0];
        grabbedObjRadius = objRadius;
      }
      pushMatrix();
      translate(mObj[0],mObj[1],mObj[2]);
      if(intersect){
        if(grab){
          stroke(objGrabColor);
        }
        else{
          stroke(objOverColor);
        }
      }
      else{
        stroke(objNormalColor);
      }
      sphere(objRadius);
      popMatrix();
    }}}
